import threading
import time
import PCA9685

YAW_CENTER_POINT = 1575
PITCH_CENTER_POINT = 1575

YAW_MIN_VALUE = 500
PITCH_MIN_VALUE = 500
YAW_MAX_VALUE = 2500
PITCH_MAX_VALUE = 2500

threadLock = threading.Lock()

def _constrain(x,xmin,xmax):
    if x < xmin: return xmin
    elif x > xmax: return xmax
    else: return x

class ServoSpeedControl(threading.Thread):
    def __init__(self,yawChannel,pitchChannel):
        threading.Thread.__init__(self)
        self.servoInstance = PCA9685.PCA9685()
        self.yawChannel = yawChannel
        self.pitchChannel = pitchChannel
        self.yawSpeed = 0
        self.pitchSpeed = 0
        self.yawMaxValue = YAW_MAX_VALUE
        self.pitchMaxValue = PITCH_MAX_VALUE
        self.yawMinValue = YAW_MIN_VALUE
        self.pitchMinValue = PITCH_MIN_VALUE
        self.isHalted = 0
    def initServo(self):
        self.servoInstance.setPWMFreq(50)
        self.servoInstance.setServoPulse(self.yawChannel,YAW_CENTER_POINT)
        self.servoInstance.setServoPulse(self.pitchChannel,PITCH_CENTER_POINT)
        self.yawPulse   = YAW_CENTER_POINT
        self.pitchPulse = PITCH_CENTER_POINT
    def resetServo(self):
        self.yawPulse = YAW_CENTER_POINT
        self.pitchPulse = PITCH_CENTER_POINT
        self.servoInstance.setServoPulse(self.yawChannel,YAW_CENTER_POINT)
        self.servoInstance.setServoPulse(self.pitchChannel,PITCH_CENTER_POINT)
    def setSpeed(self,yawSpeed,pitchSpeed):
        threadLock.acquire()
        self.yawSpeed = yawSpeed
        self.pitchSpeed = pitchSpeed
        threadLock.release()
    def halt(self):
        self.isHalted = 1
    def run(self):
        while not self.isHalted:
            self.pitchPulse = _constrain(self.pitchPulse+self.pitchSpeed,self.pitchMinValue,self.pitchMaxValue)
            self.yawPulse = _constrain(self.yawPulse+self.yawSpeed,self.yawMinValue,self.yawMaxValue)
            self.servoInstance.setServoPulse(self.yawChannel,int(self.yawPulse))
            self.servoInstance.setServoPulse(self.pitchChannel,int(self.pitchPulse))
            time.sleep(0.05)

if __name__ == '__main__':
    t = ServoSpeedControl(0,1)
    t.initServo()
    t.start()
    while True:
        t.setSpeed(-30,-30)
        time.sleep(1)
        t.setSpeed(5,5)
        time.sleep(1)
        t.setSpeed(0,0)
        time.sleep(1)
        t.setSpeed(-5,-5)
        time.sleep(1)
        t.setSpeed(30,30)
        time.sleep(1)
        t.setSpeed(0,0)
        time.sleep(1)

            

